#include <beacon/beacon.h>
#include <servo/sg90.h>


beacon::beacon* beac;
String* serial;

int pos = 0;
bool ltr = true;


bool read_string_from_serial(void)
{
  int val = Serial.read();

  while (val != -1)
  {
    if (char(val) == '\r' || char(val) == '\n')
      return true;

    (*serial) += char(val);

    val = Serial.read();
  }

  return false;
}


void loop()
{
  if (read_string_from_serial())
  {
    /**/ if ((*serial) == "Activate")
      beac->activate();
    else if ((*serial) == "Stop")
      beac->desactivate();
    else
      Serial.println("Invalid command: " + (*serial));

    (*serial) = "";
  }

  delay(100);
}

int main(void)
{
  init();

#if defined(USBCON)
  USBDevice.attach();
#endif

  Serial.begin(9600);

  beacon::beacon b(3);
  beac = &b;

  servo::sg90 s1(5);
  s1.set_position(0);

  servo::sg90 s2(6);
  s2.set_position(180);


  String ser;
  serial = &ser;

  while (true)
  {
    loop();
    if (serialEventRun) serialEventRun();
  }

  return 0;
}
